#include <vector>
#include <algorithm>
#include <ros/ros.h>
#include <cmath>
#include <math.h>
#include <deque>
#include <mutex>
#include <thread>
#include <fstream>
#include <csignal>

#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMavFrame.h>

#include <geometry_msgs/TwistStamped.h>
 
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/Bool.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Imu.h> 
#include <std_msgs/Float32MultiArray.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// #include <sensor_msgs/LaserScan.h>  qaq
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Altitude.h>
#include <mavros_msgs/ActuatorControl.h>
#include <mavros_msgs/WaypointList.h>



#include <unionsys_core.hpp>

#include <visualization_msgs/Marker.h>

#include <swarm_msgs/BoundingBoxes.h>
#include <swarm_msgs/BoundingBox.h>
//#include <swarm_msgs/gimbal_attitude.h>
#include <iostream>  
#include "UnionSys.hpp"

struct Coordinate_txt {  
    double x, y, z;  
};

RegisterMSN *unionsys_core = new RegisterMSN();

enum FSM_STATE { INIT, TAKEOFF, CRUISE, ATTACK};
std::string state_str[4] = { "INIT",
                        "TAKEOFF",
                        "CRUISE",
                        "ATTACK"};
FSM_STATE exec_state;

ros::Time last_offboard_request;

mavros_msgs::CommandBool arm_cmd;
mavros_msgs::SetMode offb_set_mode;
mavros_msgs::SetMavFrame setmavframe;

const float P_yaw = 1.00;
const float P_z = 1.00;

double last_calc_time = 0;
double last_vision_time = 0;
const float YAW_RATE_LIMIT = 15.0;
double have_target_time;
double up_45cam_update_time;

geometry_msgs::Point  error_pos_integrated, attitude_expect, velocity_expected;

//rc channels
int channel1_value;
int channel2_value;
int channel3_value;
int channel4_value;
int channel5_value;
int channel6_value;
int channel7_value;
int channel8_value;
int channel_test;

const float deg2rad = 3.1415926535798/180.0;
const float rad2deg = 180.0/3.1415926535798;

//launch file variables
double mission_number;
bool use_sitl;
double max_vel_beishu;
double zhuci_duration;

int image_width;
int image_height; 
double P_pos;
double I_pos;
double D_pos;
double I_zone;
double box_h;
double box_v;
double Bias_H;
double Bias_V;


double imu_img_delay;
double track_dist;
bool attack_air;
//count number define
int vel_command_cnt = 0;
int print_state_cnt = 0;

double ekf_predict_time = 0;

Eigen::Matrix3d R_rotaion;
Eigen::Matrix3d R_up;
float dt_vision = 0;

struct Box
{
	float x_min;
	float y_min;
	float z_min;
	float x_max;
	float y_max;
	float z_max;
	int id;
};

//ros node define

ros::Subscriber altitude_sub;
ros::Subscriber velocitysub;
ros::Subscriber local_odom_sub;
ros::Subscriber rcin_sub;
ros::Subscriber state_sub;
ros::Subscriber horizonal_target_sub, up45_target_sub;
ros::Subscriber gps_self_sub, gps_target_sub;

ros::Publisher  local_vel_pub,  marker_pub;

ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;
ros::ServiceClient set_vechile_ned;

ros::Timer px4_control_timer;

//msg define
mavros_msgs::State current_state;
geometry_msgs::Point current_point;
geometry_msgs::Point current_vel;
double vel_xyz;
geometry_msgs::Quaternion current_angle;
geometry_msgs::Vector3 curr_plane_angle;
Eigen::Quaterniond current_angle_Q;
double current_point_z = 0.0;


geometry_msgs::Point target_point;
geometry_msgs::Point attack_point;

geometry_msgs::Point last_baji_point;
double last_baji_time;
geometry_msgs::Point last_last_baji_point;
double last_last_baji_time;

geometry_msgs::Point predic_baji_vel;
geometry_msgs::Point predict_point;

std::deque<Box> target_box;
std::deque<nav_msgs::Odometry::Ptr> imu_buffer;
std::deque<nav_msgs::Odometry::Ptr> imu_buffer2;


//flag define
bool local_odom_flag = false;

Eigen::Matrix3d rotation_vehicle;

Eigen::Vector3d P_Cam;
Eigen::Vector3d P_Cam_Ekf;
Eigen::Vector3d P_Cam_Latency_Eliminate;
Eigen::Vector3d P_Cam_Track;
// need to be calibrated
double FOCUS; 
double REALSIZE;

sensor_msgs::NavSatFix self_gps_pos;
sensor_msgs::NavSatFix target_gps_pos;
double target_gps_receive = 0.0;
Eigen::Vector3d target_local_coord;
double target_yaw_angle = 0.0;
//gimbal_node
geometry_msgs::Point gimbal_curr_att;
sensor_msgs::Imu gimbal_set_att;
//function define
void position_pid_control(Eigen::Vector3d target_position, float velocity_limit, float set_yaw_angle);
void vel_command_send(geometry_msgs::Point vel, double yaw_rate);
void changeFSMExecState(FSM_STATE new_state, std::string pos_call);
void draw_box(Box temp_box, int id, double width, double red, double green, double blue);
void ekf_predict_process();

void printFSMExecState();
void set_cruise_state();
void gps_self_cb(const sensor_msgs::NavSatFix &msg);
void gps_target_cb(const sensor_msgs::NavSatFix &msg);
void horizonal_target_cb(const swarm_msgs::BoundingBoxes &msg);
void velocity_callback(const geometry_msgs::TwistStamped &msg);
void rcin_callback(const mavros_msgs::RCIn & rcvalue);
void px4_control_callback(const ros::TimerEvent& /*event*/) ;
void local_odom_callback(const nav_msgs::Odometry::Ptr & current_msg);
void altitude_callback(const mavros_msgs::Altitude &msg);
void state_cb(const mavros_msgs::State::ConstPtr& msg);

double feedforward_x = 0.0;
double feedforward_y = 0.0;

template <typename T>
T limit(T a, T max)
{
  if(a > max)
    a = max;
  if(a < -max)
    a = -max;
  return a;
}
UnionsysEKF ekf; 

// EKF Param
float dt = 0.1;
Eigen::VectorXd z_measured(3); 
float rconstant = 1;
float qconstant = 0.1;
float sys_latency = 0.8;
double error_yaw_esitmate = 0.0;

